package NursE;

import Movement.NEPilot;
import lejos.nxt.LCD;
import lejos.nxt.Motor;
import lejos.robotics.navigation.DifferentialPilot;

public class NENurse {
	
	/**
	 * ----------------------------------------------------------------
	 * 								Attributes
	 * ----------------------------------------------------------------
	 */
	
	protected NEWheel leftWheel;
	protected NEWheel rightWheel;
	protected NEHead head;
	
	protected NEPilot pilot;
	
	protected int width;
	
	/**
	 * ----------------------------------------------------------------
	 * 								Static constructor
	 * ----------------------------------------------------------------
	 */
	
	public static NENurse newNurseWithLeftAndRightWheelsAndHeadAndWidth(NEWheel leftWheel, NEWheel rightWheel, NEHead head, float width) {
		NENurse nurse = new NENurse();
		
		nurse.setLeftWheel(leftWheel);
		nurse.setRightWheel(rightWheel);
		nurse.setHead(head);
		nurse.initPilot();
		
		return nurse;
		
	}

	/**
	 * ----------------------------------------------------------------
	 * 								Setters
	 * ----------------------------------------------------------------
	 */

	public void setHead(NEHead head) {
		this.head = head;
		
	}

	public void setRightWheel(NEWheel rightWheel) {
		this.rightWheel = rightWheel;
		
	}

	public void setLeftWheel(NEWheel leftWheel) {
		this.leftWheel = leftWheel;
		
	}
	
	public void initPilot() {
		this.pilot = NEPilot.newNEPilotWithWheelDiameterWidthAndLeftAndRightMotors(this.rightWheel().diameter(), this.width(), this.leftWheel().motor(), this.rightWheel().motor());
		
	}

	/**
	 * ----------------------------------------------------------------
	 * 								Getters
	 * ----------------------------------------------------------------
	 */

	public NEHead head() {
		return this.head;
		
	}

	public NEWheel rightWheel() {
		return this.rightWheel;
		
	}

	public NEWheel leftWheel() {
		return this.leftWheel;
		
	}
	
	public float width() {
		return this.width;
		
	}
	
	/**
	 * ----------------------------------------------------------------
	 * 								Movement
	 * ----------------------------------------------------------------
	 */
	
	

	
	public void moveForward() {
		this.pilot.moveForward();
		
	}
	
	public void moveBackward() {
		this.pilot.moveBackward();
		
	}
	
	public void moveBackwardInScanning()
	{
		if(head.lookToTheRight()==false)
		{
			this.pilot.turnRight(45);
		}
		else if(head.lookToTheLeft()==false)
		{
			this.pilot.turnLeft(45);
		}
		else
		{
			this.pilot.moveBackward();
		}
	}
	
	public void stop() {
		this.pilot.stop();
	}
	
	public void move(){
		LCD.drawString("GO!",0,0 );
		LCD.refresh();
		
		  if(head.detection(20)==true) 
		  {
			  stop();
			  if(head.lookToTheRight()==true)
			  {
				  bypassesByTheRight();
			  }
			  else if(head.lookToTheLeft()==true)
			  {
				  bypassesByTheLeft();
			  }
			  else
			  {
				  moveBackwardInScanning();
			  }
		  }
		 else if(this.pilot.shockDetection()==true)
		{
			 moveBackwardInScanning();
		}   
	}
		
	
	


	
	/**
	 * ----------------------------------------------------------------
	 * 								Rotation
	 * ----------------------------------------------------------------
	 */
	
	public void turnLeft() {
		this.pilot.turnLeft();
		
	}
	
	public void turnRight() {
		this.pilot.turnRight();
		
	}
	
	/**
	 * ----------------------------------------------------------------
	 * 								ByPasses
	 * ----------------------------------------------------------------
	 */
	
	public void bypassesByTheLeft()
	{
		if(head.lookToTheLeft()==true)
		{
			//the robot turn the head to look at the right
			this.head.lookToTheFront(true);
			this.head.lookToTheRight();
			
			//he turn to the left to bypasse the obstacle
			this.pilot.turnLeft(90);
			
			//while he detect something to the right he forward
			while(head.detection(10))
			{
				this.pilot.moveForward();
			}
			
			//he surpass the obstacle 
			this.pilot.moveForward(20);
			
			//he turn right to follow the obstacle
			this.pilot.turnRight(90);
			this.pilot.moveForward(20);
			
			//while the robot detect something he forward
			while(head.detection(50)==true)
			{
				this.pilot.moveForward();
			}
			this.pilot.moveForward(20);
			
			//he turn to the right to borrow his way
			this.pilot.turnRight(90);
			this.pilot.moveForward(20);
		}	
	}
	
	public void bypassesByTheRight()
	{
		if(head.lookToTheRight()==true)
		{
			//the robot turn the head to look at the left
			this.head.lookToTheFront(false);
			this.head.lookToTheLeft();
			
			//he turn to the right to bypasse the obstacle
			this.pilot.turnRight(90);
			
			//while he detect something to the right he forward
			while(head.detection(10))
			{
				this.pilot.moveForward();
			}
			
			//he surpass the obstacle 
			this.pilot.moveForward(20);
			
			//he turn left to follow the obstacle
			this.pilot.turnLeft(90);
			this.pilot.moveForward(20);
			
			//while the robot detect something he forward
			while(head.detection(50)==true)
			{
				this.pilot.moveForward();
			}
			this.pilot.moveForward(20);
			
			//he turn to the left to borrow his way
			this.pilot.turnLeft(90);
			this.pilot.moveForward(20);
		}
		
	}
	
	

}
